/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#ifndef __AHRS_MAHONY_H__
#define __AHRS_MAHONY_H__

#include <math.h>

#include "base/types/quaternions.h"

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

///
/// \brief Madgwick's implementation of Mayhony's AHRS algorithm.
/// \see http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
///
class AHRS_Mahony
{
public:
    AHRS_Mahony();
    AHRS_Mahony(float Kp, float Ki, float sFreq);
    ~AHRS_Mahony();

    void update(float gx, float gy, float gz,
                float ax, float ay, float az,
                float mx, float my, float mz);
    void updateIMU(float gx, float gy, float gz,
                   float ax, float ay, float az);


    float twoKp;			///< 2 * proportional gain (Kp)
    float twoKi;			///< 2 * integral gain (Ki)
    float sampleFreq;       ///< sample frequency in Hz

    pi::Quaternions<float> q;   ///< quaternion of sensor frame relative to auxiliary frame

    float integralFBx,
          integralFBy,
          integralFBz;      ///< integral error terms scaled by Ki
};


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


///
/// \brief Implementation of Madgwick's IMU and AHRS algorithms.
/// \see http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
///
class AHRS_Madgwick
{
public:
    AHRS_Madgwick();
    AHRS_Madgwick(float b, float sFreq);
    ~AHRS_Madgwick();

    void update(float gx, float gy, float gz,
                float ax, float ay, float az,
                float mx, float my, float mz);
    void updateIMU(float gx, float gy, float gz,
                   float ax, float ay, float az);


    float beta;             ///< 2 * proportional gain (Kp)
    float sampleFreq;       ///< sample frequency in Hz

    pi::Quaternions<float> q;   ///< quaternion of sensor frame relative to auxiliary frame
};

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


///
/// \brief Fast inverse square-root
/// \param x - input value
/// \return inverse square-root
/// \see http://en.wikipedia.org/wiki/Fast_inverse_square_root
///
float invSqrt(float x);

#endif // end of __AHRS_MAHONY_H__
